#include "FrcRobot.h"
#include "FrcConstants.h"


FrcRobot::FrcRobot(void)
{

}

void FrcRobot::RobotInit() 
{
	
	m_ds = DriverStation::GetInstance();
	m_lcd = DriverStationLCD::GetInstance();	
	
	m_autoTimer = new Timer();
	m_frcDriveSubsystem = new FrcDriveSubsystem();
	m_frcDriveSubsystem->DoRobotInit();
	m_frcShooterSubsystem = new FrcShooterSubsystem();
	m_frcShooterSubsystem->DoRobotInit();
	
	
	m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"RobotInit() completed.");
	m_lcd->UpdateLCD();
	
	SmartDashboard::init();
}

void FrcRobot::AutonomousInit() 
{
	printf("autonomous initialized \n");  
	m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"AutonomousInit() completed.");
	m_lcd->UpdateLCD();
	// set initial state of AutoState variable
	m_autoState = kWait;
	m_frcDriveSubsystem->DoAutonomousInit();
	m_autoTimer->Reset();
	m_autoTimer->Start();
}

void FrcRobot::TeleopInit() 
{
	printf("Teleop initialized \n");
	m_frcDriveSubsystem->DoTeleopInit();
	m_frcShooterSubsystem->DoTeleopInit();
	
	m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"TeleopInit() completed.");
	m_lcd->UpdateLCD();
}

void FrcRobot::DisabledInit() 
{
	m_frcDriveSubsystem->DoDisabledInit();
	m_frcShooterSubsystem->DoDisabledInit();
	
	
	m_lcd->PrintfLine(DriverStationLCD::kUser_Line1,"DisabledInit() completed.");
	m_lcd->UpdateLCD();
}

void FrcRobot::AutonomousPeriodic() 
{
	m_frcDriveSubsystem->DoAutonomousPeriodic();
	
	switch (m_autoState)
	{
	case kWait:
		if(m_autoTimer->Get() >= c_autoDelay)
		{
			m_autoState = kMove;
		}
	break;
	
	case kMove:
		m_frcDriveSubsystem->SetTargetTimeInSeconds(5);
		m_frcDriveSubsystem->DriveByTime();
		m_autoState = kMoveCheck;
	break;
	
	case kMoveCheck:
		if (m_frcDriveSubsystem->IsDriveByTimeFinished() == true)
		{
			
			m_autoState = kDone;
		}
	break;
	
	case kDone:
		break;
		
		
	}
}
	
void FrcRobot::TeleopPeriodic() 
{

	m_frcDriveSubsystem->DoTeleopPeriodic();
	m_frcShooterSubsystem->DoTeleopPeriodic();
	
	

}

void FrcRobot::DisabledPeriodic()  
{
	m_frcDriveSubsystem->DoDisabledPeriodic();

}

START_ROBOT_CLASS(FrcRobot);
